#include "KF.h"
#include "IMU.h"
#include "main.h"
#include <math.h>

//定义加速度计的量程,单位为g
#define ACC_RANGE 8.0f
//定义陀螺仪的量程,单位为度每秒
#define GYRO_RANGE 1000.0f

uint8_t Read_Register(uint8_t reg);

void Write_Register(uint8_t reg, uint8_t value);

void IMU_Output(float *result);

void IMU_Get_Value(float *value);

//定义三轴陀螺仪的偏置值
float gyro_offset[3] = {0};

extern SPI_HandleTypeDef hspi1;


//读寄存器值
//输入参数为寄存器地址
//输出参数为寄存器值
uint8_t Read_Register(uint8_t reg)
{
    uint8_t value = 0;
    uint8_t address = 0;
    //作为读，需要将地址的最高位设置为1
    address = reg | 0x80;
	
	//进行片选
	HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_RESET);

	//发送数据
	HAL_SPI_Transmit(&hspi1,&address,1,1000);

	//接收数据
	HAL_SPI_Receive(&hspi1,&value,1,1000);

	//进行片选
	HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_SET);

    return value;
}

//写寄存器值
//输入参数为寄存器地址和寄存器值
//输出参数为空
void Write_Register(uint8_t reg, uint8_t value)
{
    uint8_t address = 0;
    //作为写，需要将地址的最高位设置为0
    address = reg & ~0x80;
	
	//进行片选
	HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_RESET);

	//发送数据
	HAL_SPI_Transmit(&hspi1,&address,1,1000);

	//接收数据
	HAL_SPI_Receive(&hspi1,&value,1,1000);

	//进行片选
	HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_SET);

}

//执行传感器初始化
void IMU_Init(void)
{

    uint8_t temp = 0;
    temp = Read_Register(0x72);

    //执行IMU初始化
    uint8_t regvalue = temp;
    //执行IMU的复位
    regvalue = 0x02;
    Write_Register(IMU_REG_MISC2, regvalue);
    HAL_Delay(100);

    //设置加速度计
    /*ACC_CONFIG寄存器配置如下
    0-3设置加速度计的采样频率
    设置为0111，对应为400Hz
    4-6设置加速度计的量程
    设置为010，对应为±8g*/
    regvalue = 0x27;
    Write_Register(IMU_ACCEL_CONFIG0, regvalue);
    HAL_Delay(10);

    //设置陀螺仪
    /*GYRO_CONFIG寄存器配置如下
    0-3设置陀螺仪的采样频率
    设置为0111，对应为400Hz
    4-7设置陀螺仪的量程
    设定为0010，对应为±1000度每秒
    */
    regvalue = 0x27;
    Write_Register(IMU_GYRO_CONFIG0, regvalue);
    HAL_Delay(10);

   //使能加速度计和陀螺仪
   //执行pwr的复位
    /*PWR寄存器配置如下
    0-1配置加速度计的状态：
        00关闭
        01关闭
        10开启低功耗模式
        11开启低噪声模式
    2-3配置陀螺仪的状态
        00关闭
        01陀螺仪处于待机模式
        10开启低功耗模式
        11开启低噪声模式*/
    regvalue = 0x0F;
    Write_Register(IMU_PWR_MGMT0, regvalue);
    HAL_Delay(100);

    //执行偏置值的初始化
    //读取一次数据
    float value[6] = {0};
    IMU_Get_Value(value);
    //将陀螺仪偏置值存储在全局变量中
    for (int i = 0; i < 3; i++)
    {
        gyro_offset[i] = value[i + 3];
    }
    //计算初始静态下的矢量合加速度
    float acc_init = 0;
    acc_init = sqrtf(value[0] * value[0] + value[1] * value[1] + value[2] * value[2]);

   //初始化卡尔曼滤波器
   KF_Init(&acc_init);
}

//获取IMU的加速度计和陀螺仪的值，需要读取12个寄存器
//输入参数为float*类型的指针，长度为6
//输出参数为空
void IMU_Get_Value(float *value)
{

//    uint8_t temp = 0;
//    temp = Read_Register(0x72);
//    R_SCI_B_UART_Write(&g_uart0_ctrl, &temp, 2);

    uint8_t reg[12];
    int16_t data[6]= {0};
    //读取IMU的加速度计和陀螺仪的值
    uint8_t reg_address = IMU_ACCEL_DATA_X1_UI;
    //读取12个寄存器的值
    for (uint8_t i = 0; i < 12; i++)
    {
        reg[i] = Read_Register(reg_address);
        reg_address++;
    }
    
    //将reg的值分高低位存储在data中
    for (int i = 0; i < 6; i++)
    {
        data[i] = (reg[2 * i + 1] << 8) | reg[2 * i];
    }

    //将加速度的值转换为float类型
    for (int i = 0; i < 3; i++)
    {
        value[i] = (float)data[i] / (32768.0f / ACC_RANGE );
    }

    //将陀螺仪的值转换为float类型
    for (int i = 3; i < 6; i++)
    {
        value[i] = (float)data[i] / (32768.0f / GYRO_RANGE);
        //将陀螺仪的值进行偏置值的补偿
        value[i] -= gyro_offset[i - 3];
    }

}

//执行结果的输出
void IMU_Output(float *result)
{
    //输出结果的具体实现
    //----------
}

//执行单次数据的更新
//输入为float数组，输出为空
void IMU_Update(float* result)
{
    float value[6] = {0};
    //获取IMU的加速度计和陀螺仪的值
    IMU_Get_Value(value);
    //执行卡尔曼滤波算法
    KF_Update(value, result);
//    //输出结果
//    IMU_Output(result);
}
